/**
 * All the functions needed for the static objects
 *
 * Originally written by: Jonas Strandstedt
 *
 * Note: I have thought of remaking so this list only contains velocity and mass etc and 
 * a link to another list containing all the meshes and textures so there are no duplicates
 */


/** 
 * addCollisionObject adds an static object to the array
 * @param char identity[] = A name of the object, appears in debugmessages
 * @param Location pos = the location
 * @param XYZ angle = The angle around the angles (CURRENTLY ONLY Z IS IN USE)
 * @param float mass = the mass of the object, above 10 is recommended
 * @param int collide = for example OBJECT_PASS_THROUGH / OBJECT_BUMP / OBJECT_IMMOVABLE /OBJECT_DISABLED
 * @param void (*f)(PlayerObject*,CollisionObject*) = pointer to a callback function
 * @param void (*animation)(CollisionObject*) = pointer to animation function
 */
int addCollisionObject(char identity[], int model, int texture, Location pos, XYZ angle, float mass, int collide, void (*collisionFunction)(PlayerObject*,CollisionObject*), void (*animation)(CollisionObject*), GLhandleARB shader) {
	
	printf("Initializing CollisionObject %s",identity);
	
	int i =0;
	while (i < COLLISIONOBJECT_ARRAY_MAX && collisionObjectArray[i] != 0) {
		i++;
	}
	if (i == COLLISIONOBJECT_ARRAY_MAX) {
		printf("... Failed to add collision object!\n");
		return -1;
	}
	CollisionObject *obj = (CollisionObject*)malloc(sizeof(CollisionObject));
	
	strcpy(obj->identifier,identity);
	
	obj->model = model;
	obj->texture = texture;
	
	obj->pos = pos;
	obj->pos_orig = pos;
	obj->angle = angle;
	
	obj->velocity.x = 0.0f;
	obj->velocity.y = 0.0f;
	obj->velocity.z = 0.0f;
	
	obj->mass = mass;
	
	obj->effect = 0;
	obj->effect_timer = 0.0f;
	
	// maybe use later if wanting to make invisible objects
	obj->visible = 1;
	
	obj->collideType = collide;
	obj->lastCollision = 0;
	
	obj->collisionFunction = collisionFunction;
	obj->animationFunction = animation;
	obj->shader = shader;
	
	collisionObjectArray[i] = obj;
	
	printf("...Done!\n\n");
	
	// returning the current number if wanting to add a texture etc
	return i;
}

/** 
 * moveCollisionObject handles the physic movement
 * @param CollisionObject *obj = The object to do the movement on
 */
void moveCollisionObject(CollisionObject *obj) {
	if (obj->animationFunction != 0 && obj->animationFunction != object_water) {
		void (*animatef)(CollisionObject*) = obj->animationFunction;
		(*animatef)(obj);
	}
	
	float friction = 0.9;
	// x
	if (obj->velocity.x < 0.0f) {
		obj->velocity.x += ((friction / obj->mass) * (6.0f / fps));							
		
	}
	if (obj->velocity.x > 0.0f) {
		obj->velocity.x -= ((friction / obj->mass) * (6.0f / fps));			
	}
	
	if (fabsf(obj->velocity.x) < 0.01f && obj->velocity.x != 0.0f) {				
		obj->velocity.x = 0.0f;
	}
	obj->pos.x += obj->velocity.x;
	
	// y
	if (obj->velocity.y < 0.0f) {
		obj->velocity.y += ((friction / obj->mass) * (6.0f / fps));							
		
	}
	if (obj->velocity.y > 0.0f) {
		obj->velocity.y -= ((friction / obj->mass) * (6.0f / fps));			
	}
	
	if (fabsf(obj->velocity.y) < 0.01f && obj->velocity.y != 0.0f) {				
		obj->velocity.y = 0.0f;
	}
	obj->pos.y += obj->velocity.y;
	
	// z
	/*
	 // no need to chek this at this point
	 if (obj->velocity.z < 0.0f) {
	 obj->velocity.z += ((friction / obj->mass) * (6.0f / fps));							
	 
	 }
	 if (obj->velocity.z > 0.0f) {
	 obj->velocity.z -= ((friction / obj->mass) * (6.0f / fps));			
	 }
	 
	 if (fabsf(obj->velocity.z) < 0.01f && obj->velocity.z != 0.0f) {				
	 obj->velocity.z = 0.0f;
	 }
	 obj->pos.x += obj->velocity.z;
	 */
}

/** 
 * bumpCheck checks if a player hits an static object and acts accordingly
 * @param PlayerObject *player = The player
 * @param CollisionObject *obj = The object to check for bump
 */
void bumpCheck(PlayerObject *player, CollisionObject *obj) {
	void (*f)(PlayerObject*,CollisionObject*) = obj->collisionFunction;
	float objRadius = modelArray[obj->model]->radius;
	float d;
	
	float xDistance = (player->pos.x + player->mesh->meshCenter.x) - (obj->pos.x + modelArray[obj->model]->meshCenter.x);
	float yDistance = (player->pos.y + player->mesh->meshCenter.y) - (obj->pos.y + modelArray[obj->model]->meshCenter.y);
	float zDistance = (player->pos.z + player->mesh->meshCenter.z) - (obj->pos.z + modelArray[obj->model]->meshCenter.z);
	d = sqrt(
			 pow(xDistance,2) + 
			 pow(yDistance,2) + 
			 pow(zDistance,2)
			 );
	if(obj->collisionFunction != 0 && d <= player->mesh->radius + objRadius){
		(*f)(player,obj);
	}
	if (obj->collideType == OBJECT_BUMP) {
		/*
		 XYZ point_vector;
		 point_vector.x = (player->pos.x - obj->pos.x)/d;
		 point_vector.y = (player->pos.y - obj->pos.y)/d;
		 point_vector.z = (player->pos.z - obj->pos.z)/d;
		 
		 XYZ angle_vector;
		 angle_vector.x = cos(player->angle);
		 angle_vector.y = sin(player->angle);
		 angle_vector.z = 0.0f;
		 
		 
		 float angle_between = acos(point_vector.x*angle_vector.x + point_vector.y*angle_vector.y + point_vector.z*angle_vector.z);
		 
		 if (d <= player->radius + obj->radius){
		 printf("angle = %f",angle_between);
		 }
		 */
		
		if(d <= player->mesh->radius + objRadius){
			float temp_velocity = player->directionalVelocity;
			
			player->directionalVelocity = (player->directionalVelocity*(player->mass - obj->mass) + 2*obj->mass*sqrt(pow(obj->velocity.x,2)+pow(obj->velocity.y,2))) / (player->mass + obj->mass);
			
			obj->velocity.x = (obj->velocity.x*(obj->mass - player->mass) + 2*player->mass*temp_velocity*cos(player->angle.z)) / (player->mass + obj->mass);
			obj->velocity.y = (obj->velocity.y*(obj->mass - player->mass) + 2*player->mass*temp_velocity*sin(player->angle.z)) / (player->mass + obj->mass);
			
			player->pos.x -= temp_velocity*cos(player->angle.z) / fps * 2;
			player->pos.y -= temp_velocity*sin(player->angle.z) / fps * 2;
		}
	} else if (obj->collideType == OBJECT_IMMOVABLE) {
		if(d <= player->mesh->radius + objRadius){
			player->pos.x -= player->directionalVelocity*cos(player->angle.z) / fps * 2;
			player->pos.y -= player->directionalVelocity*sin(player->angle.z) / fps * 2;
			player->directionalVelocity = -player->directionalVelocity*0.5;
		}
	} 

}

/** 
 * bumpCheckObjects checks if an object bumps into another
 * @param CollisionObject *obj1 = The first object to check for bump
 * @param CollisionObject *obj2 = The second object to check for bump
 */
void bumpCheckObjects(CollisionObject *obj1, CollisionObject *obj2) {
	float obj1Radius = modelArray[obj1->model]->radius;
	float obj2Radius = modelArray[obj2->model]->radius;
	float d;
	
	float xDistance = (obj1->pos.x + modelArray[obj1->model]->meshCenter.x) - (obj2->pos.x + modelArray[obj2->model]->meshCenter.x);
	float yDistance = (obj1->pos.y + modelArray[obj1->model]->meshCenter.y) - (obj2->pos.y + modelArray[obj2->model]->meshCenter.y);
	float zDistance = (obj1->pos.z + modelArray[obj1->model]->meshCenter.z) - (obj2->pos.z + modelArray[obj2->model]->meshCenter.z);
	d = sqrt(
			 pow(xDistance,2) + 
			 pow(yDistance,2) + 
			 pow(zDistance,2)
			 );
	
	if (obj1->collideType == OBJECT_BUMP && obj2->collideType == OBJECT_BUMP) {
		if(d <= obj1Radius + obj2Radius){
			float tx = obj1->velocity.x;
			float ty = obj1->velocity.y;
			obj1->velocity.x = (obj1->velocity.x*(obj1->mass - obj2->mass) + 2*obj2->mass*obj2->velocity.x) / (obj2->mass + obj1->mass);
			obj1->velocity.y = (obj1->velocity.y*(obj1->mass - obj2->mass) + 2*obj2->mass*obj2->velocity.y) / (obj2->mass + obj1->mass);
			obj2->velocity.x = (obj2->velocity.x*(obj2->mass - obj1->mass) + 2*obj1->mass*tx) / (obj2->mass + obj1->mass);
			obj2->velocity.y = (obj2->velocity.y*(obj2->mass - obj1->mass) + 2*obj1->mass*ty) / (obj2->mass + obj1->mass);
		}
	}
	
	if (obj1->collideType == OBJECT_IMMOVABLE && obj2->collideType == OBJECT_BUMP) {
		if(d <= obj1Radius + obj2Radius){
			obj2->velocity.x = -obj2->velocity.x;
			obj2->velocity.y = -obj2->velocity.y;
			obj2->velocity.z = -obj2->velocity.z;
		}
	}
	
	if (obj1->collideType == OBJECT_BUMP && obj2->collideType == OBJECT_IMMOVABLE) {
		if(d <= obj1Radius + obj2Radius){
			obj1->velocity.x = -obj1->velocity.x;
			obj1->velocity.y = -obj1->velocity.y;
			obj1->velocity.z = -obj1->velocity.z;
		}
	}
}

/**
 * drawCollisionObject
 */
void initArrays() {
	int i = 0;
	while (i<BULLET_ARRAY_MAX) {
		bulletArray[i] = 0;
		i++;
	}
	
	i = 0;
	while (i<MODEL_ARRAY_MAX) {
		modelArray[i] = 0;
		i++;
	}
	
	i = 0;
	while (i<TEXTURE_ARRAY_MAX) {
		textureArray[i] = 0;
		i++;
	}
	
	i = 0;
	while (i<COLLISIONOBJECT_ARRAY_MAX) {
		collisionObjectArray[i] = 0;
		i++;
	}
}

/**
 * drawCollisionObject
 */
int addBullet(PlayerObject *player, int model, int texture, int damage, int killAnimation, void (*animation)(Bullet*), Location pos, XYZ velocity, XYZ angle) {
	Bullet *theBullet = (Bullet*)malloc(sizeof(Bullet));
	int i =0;
	while (i < BULLET_ARRAY_MAX && bulletArray[i] != 0) {
		i++;
	}
	if (i == BULLET_ARRAY_MAX) {
		printf("Failed to add bullet!\n");
		return 1;
	}
	
	theBullet->pos = pos;
	theBullet->velocity = velocity;
	theBullet->angle = angle;
	theBullet->model = model;
	theBullet->texture = texture;
	theBullet->player = player;
	theBullet->damage = damage;
	theBullet->killAnimation = killAnimation;
	theBullet->animationFunction = animation;
	
	bulletArray[i] = theBullet;
	return 0;
}

/**
 * drawCollisionObject
 */
int removeBullet(Bullet *theBullet) {
	printf("Removing bullet");
	int i =0;
	while (i < BULLET_ARRAY_MAX && bulletArray[i] != theBullet) {
		i++;
	}
	if (i == BULLET_ARRAY_MAX) {
		printf("Failed to remove bullet!\n");
		return 1;
	}
	
	free(theBullet);
	bulletArray[i] = 0;
	
	printf("...Done!\n");
	
	return 0;
}

/**
 * drawCollisionObject
 */
int addModel(char filename[100], float scale, XYZ rotation) {
	printf("Adding model %s to array\n", filename);
	int i =0;
	while (i < MODEL_ARRAY_MAX && modelArray[i] != 0) {
		i++;
	}
	if (i == MODEL_ARRAY_MAX) {
		printf("...Failed to add model!\n");
		return -1;
	}
	
	ObjectMesh *theModel = (ObjectMesh*)malloc(sizeof(ObjectMesh));
	initObjectMesh(theModel,filename, scale, rotation);
	
	modelArray[i] = theModel;
	printf("...Done!\n\n");
	return i;
}

int removeModel(ObjectMesh *theModel) {
	printf("Removing model");
	int i =0;
	while (i < MODEL_ARRAY_MAX && modelArray[i] != theModel) {
		i++;
	}
	if (i == MODEL_ARRAY_MAX) {
		printf("Failed to remove model!\n");
		return 1;
	}
	
	free(theModel);
	modelArray[i] = 0;
	
	printf("...Done!\n");
	
	return 0;
}

int removeCollisionObject(CollisionObject *obj) {
	printf("Removing object");
	int i =0;
	while (i < COLLISIONOBJECT_ARRAY_MAX && collisionObjectArray[i] != obj) {
		i++;
	}
	if (i == COLLISIONOBJECT_ARRAY_MAX) {
		printf("Failed to remove object!\n");
		return 1;
	}
	
	free(obj);
	collisionObjectArray[i] = 0;
	
	printf("...Done!\n");
	
	return 0;
} 

/**
 * drawCollisionObject
 */
int addTexture(char filename[100]) {
	printf("Adding texture");
	int i =0;
	while (i < TEXTURE_ARRAY_MAX && textureArray[i] != 0) {
		i++;
	}
	if (i == TEXTURE_ARRAY_MAX) {
		printf("Failed to add texture!\n");
		return -1;
	}
	
	int theTexture = 0;
	
	glEnable(GL_TEXTURE_2D);
    glGenTextures(1, &theTexture);
    glBindTexture(GL_TEXTURE_2D, theTexture);
    glfwLoadTexture2D(filename, GLFW_BUILD_MIPMAPS_BIT);
    glBindTexture(GL_TEXTURE_2D, 0);
	
	textureArray[i] = theTexture;
	printf("...Done!\n");
	return i;
}